#include "differential_drive.h"
#include "Motor.h"

#define WHEEL_BASE 0.278f // 两轮间距（单位：米）
#define WHEEL_RADIUS 0.02f // 轮子半径（单位：米）

#define M_PI 3.1415926


/// @brief 初始化差速轮速度
/// @param 无 
void DifferentialDrive_Init(void)
{
    Motor_data.MeasureSpeed_Left = 0.0f;
    Motor_data.MeasureSpeed_Right = 0.0f;
}


/// @brief 根据输入的线速度 vel_x 和角速度 vel_th，算出设定值转速
/// @param vel_x 线速度
/// @param vel_th 角速度
void DifferentialDrive_Update(float vel_x, float vel_th)
{
    // 计算左右轮目标速度
    double vel_left = (vel_x - vel_th * WHEEL_BASE / 2) ;
    double vel_right = (vel_x + vel_th  * WHEEL_BASE / 2);

    double left_wheel_perimeter = 2.0 * M_PI * WHEEL_RADIUS; // 计算左轮周长
    double right_wheel_perimeter = 2.0 * M_PI * WHEEL_RADIUS; // 计算右轮周长

    Motor_data.MeasureSpeed_Left = (vel_left / left_wheel_perimeter) * 60.0; // 将m/s转换为rpm
    Motor_data.MeasureSpeed_Right = (vel_right / right_wheel_perimeter) * 60.0; // 将m/s转换为rpm

}
